%% 稳态非零参考值控制
% xd = Axd+Bud 状态向量的参考值和输入稳态相关
%% 
% 定义系统
clear;
clc;
m_sys = 1;
b_sys = 0.5;
k_sys = 1;
A = [0 1;-k_sys/m_sys -b_sys/m_sys];
B = [0;1/m_sys];
C = [1 0];
D = 0;
n = size(A,1);
p = size(B,2);
%% 离散化系统
Ts = 0.1;
sys_d = c2d(ss(A,B,C,D),Ts);
A = sys_d.a;
B = sys_d.b;

%%%%%权重设计%%%%%%%%
Q = [1 0; 0 1];%n*n
S = [1 0; 0 1];%n*n
R = 1;%p*p
%%%%%%系统参考值%%%%%%
xd = [1;0];
AD = eye(n);

x0 = [0;0];
x = x0;
xa = [x;xd];
u = 0;

k_steps = 100;
x_history = zeros(n,k_steps);
u_history = zeros(p,k_steps);

[Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd);
[F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa);

for k = 1:k_steps
    u = -F*xa+ud;
    x = A*x+B*u;
    xa = [x;xd];
    x_history(:,k+1)=x;
    u_history(:,k)=u;  
end
%% 制图
figure; % 创建一个新的图形窗口
subplot(2,1,1);% 创建两个子图，2行1列，当前激活第一个子图
hold;
plot(0:length(x_history)-1,x_history(2,:),'--');
plot(0:length(x_history)-1,x_history(1,:),'-');
grid on
legend("x2","x1")%图例
hold off;
xlim([0 k_steps]);%设置x轴的显示氛围
ylim([-0.2 1.2]);
% 系统输入结果图
subplot (2, 1, 2);%切换到第二个子图
% 系统输入结果图，施加在质量块上的力，f
% stairs (0:length(u_history)-1,u_history(1,:));
plot (0:length(u_history)-1,u_history(1,:));
legend("u")
grid on
xlim([0 k_steps]);
ylim([0 3]);














